import time

sensor_log =[0]*4 # FRBL

INF = 400
HIT = 150
LMAX = 200
LMIN = 100

def get_F():
    global sensor_log
    return sensor_log[0]

def get_L():
    global sensor_log
    # print("L=",sensor_log[3])
    return sensor_log[3]

def legal_L_dis():
    L = get_L()
    # print("L=",L)
    return LMIN-20<=L and L<=LMAX+20

def not_hit_F():
    F = get_F()
    return F>HIT

def init_adjust(ep_chassis):
    while True:
        L = get_L()
        if (legal_L_dis()):break
        if L<LMIN:
            ep_chassis.move(0,0.1,0,0.3,0).wait_for_completed()
        else:
            ep_chassis.move(0,-0.1,0,0.3,0).wait_for_completed()
    
def print_sensor():
    global sensor_log
    # print("[+ Sim] F:{0}".format(sensor_log[0]))
    print("============> F:{0}  R:{1}  B:{2}  L:{3}".format(
        sensor_log[0], sensor_log[1], sensor_log[2], sensor_log[3]))

def sub_sensor_info_handler(sub_info):
    global sensor_log
    # print(sub_info)
    sensor_log = sub_info[:]

def turn_right(ep_chassis):
    print_sensor()
    print("R")
    ep_chassis.move(0,0,-90,0,60).wait_for_completed()
    time.sleep(0.5)

def move_adjust(ep_chassis):
    print_sensor()
    print("adjust")

    while not legal_L_dis():
        L = get_L()
        if (L<LMIN):
            print("near")
            ep_chassis.move(0,0,-8,0,60).wait_for_completed()
            ep_chassis.move(0,0.1,0,0.6,0).wait_for_completed()
            time.sleep(0.5)
        elif L>LMAX and L<INF:
            print("far")
            ep_chassis.move(0,0,+5,0,60).wait_for_completed()
            ep_chassis.move(0,-0.1,0,0.6,0).wait_for_completed()
            time.sleep(0.5)
        else:
            break

def turn_left(ep_chassis):
    print_sensor()
    print("L_w")
    ep_chassis.move(0.3,0,0,0.6,60).wait_for_completed()
    print("L_L")
    ep_chassis.move(0,0,90,0,60).wait_for_completed()
    print("L_w")
    ep_chassis.move(0.5,0,0,0.6,60).wait_for_completed()
    print("L_end")
    time.sleep(0.5)
    print_sensor()


def go_map(ep_chassis):
    action = 12
    init_adjust(ep_chassis)
    while True:
        if action==0: break
        ep_chassis.drive_speed(0.2,0,0)
        print("w")
        # action-=1
        while(not_hit_F() and legal_L_dis()):
            time.sleep(0.05)
            continue
        
        print_sensor()

        ep_chassis.drive_speed(0,0,0)
        time.sleep(0.1)

        ep_chassis.move(0,0,0,0,0).wait_for_completed()

        if (not legal_L_dis()):
            print("L=",get_L())
            if get_L()>INF:
                turn_left(ep_chassis)
                action-=1
            else:
                move_adjust(ep_chassis)
        else:
            turn_right(ep_chassis)
            action-=1

        print(action)
    
    print("end")
            
def sensor_test(ep_robot):
    print("=====> start _map_go check")
    ep_robot.led.set_led(comp="all", r=0, g=200, b=0)
    
    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(freq=10, callback=sub_sensor_info_handler)

    for i in range(10):
        time.sleep(1)
        print_sensor()

    
    time.sleep(1)
    ep_sensor.unsub_distance()
    
    ep_robot.led.set_led(comp="all", r=200, g=200, b=200)
    
    print("=====> finish _map_go check")

def test_go_straigt(ep_robot):
    ep_chassis = ep_robot.chassis
    global sensor_log

    # ep_chassis.move(-1,0,0,0.7,0).wait_for_completed()
    time.sleep(1)
    # print_sensor()
    print("go")

    ep_chassis.drive_speed(0.2,0,0)

    while(sensor_log[0]>400):
        time.sleep(0.05)
        continue
        
    print_sensor()
    od = sensor_log[0]
    print("trigger")

    ep_chassis.drive_speed(0,0,0)
    time.sleep(0.1)
    print_sensor()

    ep_chassis.move(0,0,0,0,0).wait_for_completed()
    time.sleep(0.1)
    print_sensor()
    print("stop")
    nd = sensor_log[0]

    ep_chassis.drive_speed(0,0,0)
    print("stop_delay = ",od-nd)

def action_control(ep_robot):
    ep_chassis = ep_robot.chassis
    print("=====> start _map_go check")
    ep_robot.led.set_led(comp="all", r=0, g=200, b=0)
    
    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(freq=10, callback=sub_sensor_info_handler)

    time.sleep(1)
    print_sensor()
    global sensor_log
    
    go_map(ep_chassis)
    # test_go_straigt(ep_robot)

    time.sleep(0.5)
    ep_sensor.unsub_distance()
    
    ep_robot.led.set_led(comp="all", r=200, g=200, b=200)
    
    print("=====> finish _map_go check")

position =[0]*2
def print_pos():
    global position
    # print("[+ Sim] F:{0}".format(sensor_log[0]))
    print("============> x:{:.2f} y:{:.2f}".format(
        position[0], position[1]))

def sub_position_info_handler(sub_info):
    global position
    position = [sub_info[0]*100,sub_info[1]*100]

def lab(ep_robot):
    ep_chassis = ep_robot.chassis
    ep_chassis.sub_position(cs=1,freq=10, callback=sub_sensor_info_handler)

    for _ in range(30):
        print_pos()
        time.sleep(1)
    ep_chassis.unsub_position()
    ep_robot.led.set_led(comp="all", r=200, g=200, b=200)

def run(ep_robot):
    # ep_chassis = ep_robot.chassis
    # ep_chassis._dij_drive_speed(0,0,0,timeout=20)
    # time.sleep(20)
    # lab(ep_robot)

    # sensor_test(ep_robot)

    action_control(ep_robot)

    print("total end")
    return 